/*
 *  comm.c
 *  
 *
 *  Created by Gregory Koch on 27/05/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

#include "udp.h"
#include "comm.h"


img_receiver_t open_img_receiver(int port, cvSize_t size, int img_type)
{
    img_receiver_t recv;
    init_server(&recv.serv, 5000);
    
    recv.imgs[0] = init_image(img_type, size);
    recv.imgs[1] = init_image(img_type, size);
    
    recv.last_recv = 0;
    recv.cur = 0;
    recv.nb_line_next = 0;
    
    return recv;
}

void close_img_receiver(img_receiver_t recv)
{
    close_server(&recv.serv);
    
    free_image(recv.imgs[0]);
    free_image(recv.imgs[1]);
    free_image(recv.imgs[2]);
}


int send_img(image_t img, unsigned int id, udp_server_t* serv, char* ip, int port)
{
    unsigned int i;
    int sent, ret;
    
    struct sockaddr_in to = init_sockaddr_in(ip, port);
    
    char *buf = malloc(img.stride + sizeof(unsigned int)*2);
    memset(buf, 0, img.stride + sizeof(unsigned int)*2);
    memcpy(buf, &id, sizeof(unsigned int));
    
    
    for(i = 0; i<img.size.height; i++)
    {
        memcpy(buf + sizeof(unsigned int), &i, sizeof(unsigned int));
        memcpy(buf + 2 * sizeof(unsigned int), img.data + img.stride * i, img.stride);
        sent = 0;
        do
        {
            ret = send_to(serv, to, buf + sent, img.stride + 2*sizeof(unsigned int) - sent);
            
            if(ret > 0)
            {
                sent += ret;
            }
            else
            {
                free(buf);
                return ret;
            }
        } while(sent != img.stride + 2*sizeof(unsigned int));
        
    }
    
    free(buf);
    return 1;
}


int recv_img(img_receiver_t *receiver)
{
    fd_set readfs;
    int maxSock, err;
    struct timeval tv;
    int i;
    int nb_err = 0;
    int already_read;
    int recv, ret, timeout, start;
    unsigned int ref_id, id, line;
    
    char *buf = malloc(receiver->imgs[receiver->cur].stride + sizeof(unsigned int)*2);
    
    start = 0;
    
    already_read = receiver->nb_line_next;
    receiver->nb_line_next = 0;
    
    for(i = 0; i<receiver->imgs[receiver->cur].size.height; i++)
    {
        recv = 0;
        
        while(recv != receiver->imgs[receiver->cur].stride + 2 * sizeof(unsigned int))
        {
            timeout = 0;
            do
            {
                maxSock = 0;
                
                FD_ZERO (&readfs);
                FD_SET (receiver->serv.s_ecoute, &readfs);
                
                maxSock = receiver->serv.s_ecoute > maxSock ? 
                receiver->serv.s_ecoute : maxSock;
                
                tv.tv_sec = 2;
                tv.tv_usec = 0;
                
                err = select (maxSock + 1, &readfs, NULL, NULL, &tv);
                
                switch (err)
                {
                    case 0:
                        /* timeout */
                        if(start)
                        {
                            receiver->last_recv = receiver->cur;
                            receiver->cur = (receiver->cur + 1) % 2;
                            free(buf);
                            return -1;
                        }
                        timeout = 1;
                        break;
                        
                    case -1:
                        /* error */
                        receiver->last_recv = receiver->cur;
                        receiver->cur = (receiver->cur + 1) % 2;
                        free(buf);
                        return -2;
                        break;
                        
                    default:				
                        if (FD_ISSET (receiver->serv.s_ecoute, &readfs))
                        {
                            ret = recv_from(&receiver->serv);
                            
                            if(ret > 0 && recv + ret <= receiver->imgs[receiver->cur].stride + 2 * sizeof(unsigned int))
                            {
                                memcpy(buf + recv, receiver->serv.buf, ret);
                                recv += ret;
                            }
                            else
                            {
                                receiver->last_recv = receiver->cur;
                                receiver->cur = (receiver->cur + 1) % 2;
                                free(buf);
                                return -3;
                            }
                            timeout = 0;
                        }  
                }            
            } while(timeout);
            
            
        }
        
        id = (unsigned int)*(unsigned int*)buf;
        line = (unsigned int)*(unsigned int*)(buf + sizeof(unsigned int));
        
        if(i == 0)
        {
            ref_id = id;
            start = 1;
        }
        
        if(id == ref_id + 1)
        {
            memcpy(receiver->imgs[(receiver->cur + 1) % 2].data + line * receiver->imgs[(receiver->cur + 1) % 2].stride, 
                   buf + 2*sizeof(unsigned int), receiver->imgs[(receiver->cur + 1) % 2].stride);
            receiver->nb_line_next++;
            
            nb_err++;
            
            if(nb_err > 10)
            {
                receiver->last_recv = receiver->cur;
                receiver->cur = (receiver->cur + 1) % 2;
                free(buf);
                return -6; 
            }
        }
        if(id == ref_id - 1)
        {
            /* Do nothing */
        }
        else if(id != ref_id)
        {
            receiver->last_recv = receiver->cur;
            receiver->cur = (receiver->cur + 1) % 2;
            free(buf);
            return -4;
        }
        
        if(line < receiver->imgs[receiver->cur].size.height)
        {
            memcpy(receiver->imgs[receiver->cur].data + line * receiver->imgs[receiver->cur].stride, 
                   buf + 2*sizeof(unsigned int), receiver->imgs[receiver->cur].stride);
        }
        else
        {
            receiver->last_recv = receiver->cur;
            receiver->cur = (receiver->cur + 1) % 2;
            free(buf);
            return -5;
        }
    }
    
    receiver->last_recv = receiver->cur;
    receiver->cur = (receiver->cur + 1) % 2;
    return 1;
}


int send_gps_gga(gga_t *gga, udp_server_t* serv, char* ip, int port)
{
    int sent, ret;
    
    struct sockaddr_in to = init_sockaddr_in(ip, port);
    
    sent = 0;
    while(sent != sizeof(gga_t))
    {
        ret = send_to(serv, to, (char*)gga + sent, sizeof(gga_t) - sent);
        
        if(ret > 0)
        {
            sent += ret;
        }
        else
        {
            return ret;
        }
    }
    
    return 1;
}

int recv_gps_gga(gga_t *gga, udp_server_t* serv)
{
    fd_set readfs;
    int maxSock, err;
    struct timeval tv;
    
    int recv, ret, timeout, start;
    
    recv = 0;
    start = 0;
    
    timeout = 0;
    do
    {
        maxSock = 0;
        
        FD_ZERO (&readfs);    
        FD_SET (serv->s_ecoute, &readfs);
        
        maxSock = serv->s_ecoute > maxSock ? 
        serv->s_ecoute : maxSock;
        
        tv.tv_sec = 2;
        tv.tv_usec = 0;
        
        err = select (maxSock + 1, &readfs, NULL, NULL, &tv);
        
        switch (err)
        {
            case 0:
                /* timeout */
                if(start)
                {
                    return -1;
                }
                timeout = 1;
                break;
                
            case -1:
                /* error */
                return -1;
                break;
                
            default:				
                if (FD_ISSET (serv->s_ecoute, &readfs))
                {
                    ret = recv_from(serv);
                    
                    if(ret == sizeof(gga_t))
                    {
                        memcpy(gga + recv, serv->buf, ret);
                        recv += ret;
                    }
                    else
                    {
                        return -1;
                    }
                    timeout = 0;
                }
        }
        
    } while(timeout);
    
    return recv;
}

